#include "../include/BTservice/NavService.h"

namespace nav
{

    void NavService::navRequestCB(const robots_msgs::NavConstPtr &nav_order_ptr)
    {

        robots_msgs::Nav order = *nav_order_ptr;
        move_base_msgs::MoveBaseGoal goal;
        goal.target_pose.header.frame_id = "/camera_init";
        goal.target_pose.header.stamp = ros::Time::now();

        ROS_INFO("Nav service: mode: %d, x,y: %f, %f",
                 order.nav_mode, order.target_x, order.target_y);

        switch ((NavMode)order.nav_mode)
        {
        case NavMode::STOP:
        {
            if (last_nav_order.nav_mode != (int)NavMode::STOP)
                movebase_cient->cancelGoal();
            break;
        }
        case NavMode::SINGLE_POINT:
        {
            geometry_msgs::PointStamped in, out;
            in.header.frame_id = "/camera_init";
            in.header.stamp = ros::Time::now();
            in.point.x = order.target_x;
            in.point.y = order.target_y;
            in.point.z = 0;
            // listener.transformPoint("/map", in, out);

            goal.target_pose.pose.position.x = in.point.x;
            goal.target_pose.pose.position.y = in.point.y;
            goal.target_pose.pose.orientation.w = 1;
            movebase_cient->sendGoal(goal);
            break;
        }
        case NavMode::PATROLLING:
        {

            const NavPointList &list = patrolling_list[(int)order.nav_mode];

            if (order.nav_mode != last_nav_order.nav_mode)
            {
                last_patrolling_index = 0;
                last_patrolling_timestamp = ros::Time::now();

                goal.target_pose.pose.position.x = list[last_patrolling_index].x;
                goal.target_pose.pose.position.y = list[last_patrolling_index].y;
                goal.target_pose.pose.orientation.w = 1;
                movebase_cient->sendGoal(goal);
            }
            else
            {
                if (ros::Time::now() - last_patrolling_timestamp > ros::Duration(10.) || nav_status.status == nav_status.SUCCESS)
                {
                    last_patrolling_index = (last_patrolling_index == list.size() - 1) ? 0 : last_patrolling_index + 1;

                    goal.target_pose.pose.position.x = list[last_patrolling_index].x;
                    goal.target_pose.pose.position.y = list[last_patrolling_index].y;
                    goal.target_pose.pose.orientation.w = 1;
                    movebase_cient->sendGoal(goal);
                    last_patrolling_timestamp = ros::Time::now();
                }
            }
            break;
        }
        // case NavMode::SINGLE_POINT:{

        //     const NavPointList& points = single_point_list[(int)order.nav_point];
        //     if(order.nav_mode != last_nav_order.nav_mode || order.nav_point != last_nav_order.nav_point){
        //         last_single_point_index = 0;
        //         last_single_point_timestamp = ros::Time::now();
        //         goal.target_pose.pose.position.x = points[last_single_point_index].x;
        //         goal.target_pose.pose.position.y = points[last_single_point_index].y;
        //         goal.target_pose.pose.orientation.w = 1;
        //         movebase_cient->sendGoal(goal);
        //     }
        //     else {
        //         if(ros::Time::now() - last_single_point_timestamp > ros::Duration(20.) &&
        //             nav_status.status == nav_status.RUNNING){
        //             last_single_point_index = (last_single_point_index == points.size() - 1)?
        //                 0 : last_single_point_index + 1;
        //             goal.target_pose.pose.position.x = points[last_single_point_index].x;
        //             goal.target_pose.pose.position.y = points[last_single_point_index].y;
        //             goal.target_pose.pose.orientation.w = 1;
        //             movebase_cient->sendGoal(goal);
        //             last_single_point_timestamp = ros::Time::now();
        //         }
        //     }
        // }
        default:
            break;
        }
        last_nav_order = order;
    }

    NavService::NavService()
    {

        nav_cmd_sub = nh.subscribe<robots_msgs::Nav>("/NavRequest", 1, &NavService::navRequestCB, this);
        nav_status_sub = nh.subscribe<robots_msgs::NavStatus>("/nav_status", 1, &NavService::navStatusCB, this);

        last_nav_order.nav_mode = last_nav_order.MODE_STOP;
        last_patrolling_timestamp = ros::Time::now();
        last_patrolling_index = 0;
        last_single_point_timestamp = ros::Time::now();
        last_single_point_index = 0;

        movebase_cient = std::make_unique<MoveBaseClient>("move_base", true);
        if (!movebase_cient->waitForServer(ros::Duration(5)))
        {
            ROS_INFO("Can't connected to move base server");
        }
        ROS_INFO("Connected to move base server");
    }

}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "NavService");

    nav::NavService ns;
    ros::spin();
    return 0;
}